在研究移動(dòng)機(jī)器人控制算法和多機(jī)器人協(xié)作控制算法時(shí)，常采用仿真軟件來(lái)提高算法的開(kāi)發(fā)效率并減少實(shí)驗(yàn)成本。仿真軟件可直接利用物理引擎對(duì)剛體對(duì)象的運(yùn)動(dòng)進(jìn)行仿真，物理引擎為剛體賦予真實(shí)的物理屬性，以此對(duì)物體的運(yùn)動(dòng)、旋轉(zhuǎn)和碰撞進(jìn)行仿真計(jì)算。目前主流物理引擎有Havok(Havok Game Dynamics)、PhysX(Physics Engine Middleware)、Bullet（Bullet Physics Engine）和ODE（Open Dynamics Engine）等[1]，Havok是業(yè)界功能最全面的物理仿真解決方案，主要應(yīng)用于實(shí)時(shí)碰撞計(jì)算、動(dòng)力學(xué)約束求解和車(chē)輛綜合解決方案等領(lǐng)域；PhysX的特點(diǎn)是適合計(jì)算量比較大的物理模擬計(jì)算；Bullet的特點(diǎn)是具有最先進(jìn)的碰撞檢測(cè)技術(shù)，主要用于柔體動(dòng)力學(xué)和剛體動(dòng)力學(xué)的仿真；ODE的特點(diǎn)是具有高級(jí)的關(guān)節(jié)類(lèi)型和碰撞檢測(cè)整合系統(tǒng)，它適合于對(duì)機(jī)電一體化設(shè)備和移動(dòng)機(jī)器人的模擬，并且易于與C/C++ API配合使用，具有快速性、強(qiáng)健性和可移植性等特點(diǎn)[2]。
      在采用物理引擎生成動(dòng)力學(xué)世界之后，需利用圖形繪制程序?qū)?chǎng)景繪制到顯示終端[3]。常用的圖形繪制函數(shù)庫(kù)有OpenGL(Open Graphics Library)、DirectX(Direct Extension)和Irrlicht(Irrlicht Engine)等，OpenGL主要應(yīng)用于專(zhuān)業(yè)高端繪圖領(lǐng)域[4]，具有使用簡(jiǎn)便、效率高和可移植等特點(diǎn)；DirectX是微軟開(kāi)發(fā)的三維繪圖軟件包，性能上已經(jīng)超過(guò)OpenGL，但其接口采用COM方式；Irrlicht為高性能實(shí)時(shí)3D引擎，它可以提供在商業(yè)級(jí)3D引擎上才具有的藝術(shù)特性。
      綜上所述，在不影響軟件性能的前提下，為了縮短仿真軟件的開(kāi)發(fā)周期，本文采用ODE物理引擎和OpenGL來(lái)開(kāi)發(fā)移動(dòng)機(jī)器人三維仿真軟件。
1 軟件特性
       (1)采用基于面向?qū)ο蠹夹g(shù)實(shí)現(xiàn)，軟件操作簡(jiǎn)單，易于維護(hù)和功能擴(kuò)展；
       (2)可以導(dǎo)入x格式和3ds格式的三維模型文件；
       (3)允許物體同時(shí)實(shí)現(xiàn)多個(gè)移動(dòng)操作，在每個(gè)運(yùn)動(dòng)方向都有加速度、減速度和最大速度等運(yùn)動(dòng)屬性；實(shí)現(xiàn)碰撞檢測(cè)、移動(dòng)機(jī)器人和虛擬場(chǎng)景的圖形化顯示；
       (4)支持實(shí)時(shí)調(diào)試功能； 3D動(dòng)畫(huà)和仿真計(jì)算結(jié)果同步且真實(shí)對(duì)應(yīng);繪制仿真環(huán)境的二維地圖和物體運(yùn)動(dòng)軌跡。
       (5)提供與外部軟件連接的接口，即可以通過(guò)ODBC與外部數(shù)據(jù)庫(kù)相連，或通過(guò)Socket接口與外部設(shè)備相連，實(shí)現(xiàn)進(jìn)程之間的通信；
       (6)軟件接口的多樣性和擴(kuò)展性,即可通過(guò)游戲手柄、鍵盤(pán)來(lái)輸入控制信息、模型參數(shù)和仿真參數(shù)等；仿真數(shù)據(jù)的保存輸出。
       其中，速度比例系數(shù)k=0.035，線速度單位為m/s，角速度單位為rad/s。 

2.2 三維物體建模

       ODE物理引擎提供球體、盒子、膠囊、平面和圓柱等幾何體。在創(chuàng)建出一個(gè)幾何體后，其中心一般落在仿真環(huán)境坐標(biāo)系的原點(diǎn)上。在對(duì)移動(dòng)機(jī)器人進(jìn)行建模時(shí)，需要使用盒子和圓柱兩種幾何體，盒子需要指定3個(gè)參數(shù)，即長(zhǎng)、高、寬;對(duì)于圓柱而言，則需指定長(zhǎng)度和半徑2個(gè)參數(shù)。在動(dòng)力學(xué)世界中，以Geom代表物體幾何體，以Body代表虛擬場(chǎng)景中的對(duì)象。軟件可以通過(guò)調(diào)用ODE內(nèi)部函數(shù)來(lái)檢測(cè)幾何體和對(duì)象的對(duì)應(yīng)關(guān)系，也有函數(shù)用于檢測(cè)對(duì)象之間是否存在連接。下面以創(chuàng)建盒子物體為例，說(shuō)明單個(gè)物體的建模方法。
       首先調(diào)用dBodyCreate函數(shù)創(chuàng)建出給定空間中的剛體對(duì)象,再調(diào)用dBodySetPosition和dBodySetRotation兩個(gè)函數(shù)，調(diào)整該對(duì)象在空間中的位姿,接著調(diào)用dMassSetBoxTotal和dBodySetMass兩個(gè)函數(shù)設(shè)定該對(duì)象的質(zhì)量屬性,最后調(diào)用dCreateBox函數(shù)創(chuàng)建相應(yīng)尺寸的盒子幾何體,并調(diào)用dGeomSetBody函數(shù)將該幾何體與對(duì)象關(guān)聯(lián)起來(lái)。
       在創(chuàng)建出單個(gè)對(duì)象后，往往需要利用各種關(guān)節(jié)將不同對(duì)象連接起來(lái)。ODE物理引擎提供5種類(lèi)型的關(guān)節(jié)，分別為鉸鏈型、球-球窩型、滑竿柱型、固定型和鉸鏈2型等。其中鉸鏈型為合頁(yè)關(guān)節(jié)，滑竿柱型為插銷(xiāo)關(guān)節(jié)，鉸鏈2型則是帶有軸的關(guān)節(jié)，這些關(guān)節(jié)都有內(nèi)置的馬達(dá)。本文選用鉸鏈關(guān)節(jié)來(lái)連接驅(qū)動(dòng)輪和機(jī)器人車(chē)體，采用固定關(guān)節(jié)構(gòu)建機(jī)器人車(chē)體結(jié)構(gòu)。下面以使用鉸鏈關(guān)節(jié)連接兩個(gè)對(duì)象為例，說(shuō)明創(chuàng)建關(guān)節(jié)的方法。
       在調(diào)用dJointCreateHinge函數(shù)創(chuàng)建鉸鏈關(guān)節(jié)對(duì)象后，再調(diào)用dJointAttach函數(shù)指定用該關(guān)節(jié)連接的兩個(gè)物體對(duì)象，然后調(diào)用dJointSetHingeAnchor函數(shù)設(shè)定旋轉(zhuǎn)軸的中心點(diǎn)坐標(biāo)，并調(diào)用dJointSetHingeAxis設(shè)定旋轉(zhuǎn)軸的方向。

      為了讓剛體對(duì)象能夠在仿真環(huán)境中運(yùn)動(dòng)起來(lái)，ODE提供了3種方法：

      (1)調(diào)用dBodyAddForce、dBodyAddTorque等函數(shù)給剛體施加力的作用;

      (2)調(diào)用dJointSetHingeParam函數(shù)來(lái)改變內(nèi)置馬達(dá)的轉(zhuǎn)速，同時(shí)需指定該函數(shù)的第二個(gè)輸入?yún)?shù)為dParamVel; 

      (3)調(diào)用dBodySetLinearVel和dBodySetAngularVel兩個(gè)函數(shù)直接給物體設(shè)定線速度和角速度。

       此外，在ODE仿真環(huán)境中，可通過(guò)兩種方式來(lái)模擬彈簧-阻尼系統(tǒng): (1)通過(guò)設(shè)置ERP（Error Reduction Parameter）和CFM（Constraint Force Mixing）兩個(gè)參數(shù)來(lái)實(shí)現(xiàn)，ERP為每一仿真循環(huán)中的修正誤差，取值范圍為0~0.8，默認(rèn)取值為0.2；CFM代表物理引擎的全局混合約束力，它反映物體表面的柔軟程度，其取值范圍為10e-9~1；(2)利用動(dòng)力學(xué)方程來(lái)求解，即胡克定律:
       其中γ為阻尼系數(shù),它與物體的形狀以及周?chē)再|(zhì)有關(guān)。
       綜上所述，典型ODE仿真過(guò)程為[6]：
       (1)生成一個(gè)動(dòng)力學(xué)世界，并在該世界中創(chuàng)建物體；
       (2)設(shè)置物體狀態(tài)（如質(zhì)量、質(zhì)心位置和姿態(tài)等），并在動(dòng)力學(xué)世界中創(chuàng)建關(guān)節(jié)；
       (3)將關(guān)節(jié)與物體綁定起來(lái),為所有的關(guān)節(jié)設(shè)置參數(shù)；
       (4)生成碰撞檢測(cè)空間，并為需要作碰撞檢測(cè)的物體生成碰撞幾何體；創(chuàng)建一個(gè)容納關(guān)節(jié)的關(guān)節(jié)組；

      (5)循環(huán)處理過(guò)程:

      ①在物體上施加力；

       ②根據(jù)需要調(diào)整關(guān)節(jié)參數(shù)；
       ③調(diào)用碰撞檢測(cè)，得到碰撞點(diǎn)和碰撞的物體；
       ④為每個(gè)碰撞點(diǎn)的碰撞創(chuàng)建一個(gè)接觸關(guān)節(jié)，并將其放入關(guān)節(jié)組；
       ⑤執(zhí)行一個(gè)仿真步驟；
       ⑥清空接觸關(guān)節(jié)組中的關(guān)節(jié)；
       (6)銷(xiāo)毀動(dòng)力學(xué)世界和碰撞世界。
2.3 三維圖形的繪制
       OpenGL繪制圖形的基本操作步驟[4]：
       (1)設(shè)置像素格式：設(shè)定OpenGL繪制風(fēng)格、顏色模式和顏色位數(shù)等重要信息。
       (2)建立模型：根據(jù)基本圖元建立景物的三維模型，并對(duì)模型進(jìn)行數(shù)學(xué)描述。
       (3)舞臺(tái)布置：把景物放置在三維空間的適當(dāng)位置，設(shè)置視點(diǎn)、視角和投影模型等。
       (4)效果處理：設(shè)置物體的材質(zhì)，加入光照及光照條件。
       (5)光柵化：把景物及其顏色信息轉(zhuǎn)化為可在計(jì)算機(jī)屏幕上顯示的像素信息。
       在繪制圖形時(shí)，需注意坐標(biāo)系的變換，否則很容易導(dǎo)致繪制失敗。OpenGL定義了兩個(gè)坐標(biāo)系：世界坐標(biāo)系和當(dāng)前繪圖坐標(biāo)系。世界坐標(biāo)系是固定不變的，規(guī)定以屏幕中心為原點(diǎn)，面對(duì)顯示終端，向右為x正軸，向上為y正軸，向終端外面為z正軸。當(dāng)前繪圖坐標(biāo)系是繪制物體時(shí)的參考坐標(biāo)系。仿真軟件完成初始化后，世界坐標(biāo)系與當(dāng)前繪圖坐標(biāo)系是重合的。在調(diào)用glTranslatef、glRotate等變換函數(shù)對(duì)繪圖坐標(biāo)系進(jìn)行平移和旋轉(zhuǎn)后，繪圖坐標(biāo)系會(huì)在原來(lái)的基礎(chǔ)上做出相應(yīng)改變。此時(shí)，若調(diào)用gluSphere、glVertex3f等繪圖函數(shù)，繪圖函數(shù)是在改變之后的繪圖坐標(biāo)系上進(jìn)行繪制。如若要讓繪圖坐標(biāo)系與世界坐標(biāo)系再次重合，可以調(diào)用glLoadIdentity函數(shù)。此外，可以調(diào)用glColor3f（r，g，b）函數(shù)設(shè)置繪圖函數(shù)所使用的顏色，如果沒(méi)有再次調(diào)用該函數(shù)，則繪制出的圖形顏色將保持原先顏色不變，rgb三個(gè)顏色分量的取值范圍為0.0~1.0。

3 軟件框架
       軟件框架及其處理流程如圖2所示，軟件仿真循環(huán)的處理流程如圖3所示，下面簡(jiǎn)述主要處理過(guò)程的實(shí)現(xiàn)思路及方法：
       (1)軟件的初始化
       軟件的初始化工作主要包括：從配置文件讀入移動(dòng)機(jī)器人的數(shù)量、軟件運(yùn)行時(shí)所需文件的路徑和繪制窗口大小等參數(shù)；導(dǎo)入虛擬場(chǎng)景所需的素材，如聲音、紋理和3ds模型等，本文采用lib3ds庫(kù)來(lái)讀取3ds文件[6]；初始化ODBC 接口，獲取ODBC 環(huán)境句柄，實(shí)現(xiàn)進(jìn)程與數(shù)據(jù)庫(kù)的數(shù)據(jù)交互；初始化游戲手柄和UDP通信網(wǎng)絡(luò)；初始化ODE環(huán)境，創(chuàng)建動(dòng)力學(xué)環(huán)境和碰撞檢測(cè)空間，創(chuàng)建移動(dòng)機(jī)器人和仿真場(chǎng)景。
       (2)物體運(yùn)動(dòng)參數(shù)的接收
       在仿真循環(huán)中，利用UDP協(xié)議實(shí)現(xiàn)本地進(jìn)程與其他進(jìn)程之間的通信，該軟件也可以從鍵盤(pán)、游戲手柄和網(wǎng)絡(luò)數(shù)據(jù)庫(kù)讀入機(jī)器人的動(dòng)作指令。
       (3)物體碰撞檢測(cè)
       本文將與物理引擎有關(guān)的操作函數(shù)封裝成動(dòng)態(tài)鏈接庫(kù)，通過(guò)使用動(dòng)態(tài)鏈接庫(kù)技術(shù)，軟件可以實(shí)現(xiàn)模塊化，即由相對(duì)獨(dú)立的組件來(lái)組成整個(gè)軟件。這簡(jiǎn)化了軟件項(xiàng)目的管理，而且能夠節(jié)省內(nèi)存，也有助于資源共享和代碼更新移植。
       ODE 的碰撞檢測(cè)引擎需要給定兩個(gè)物體的形狀信息。在每一仿真循環(huán)中，調(diào)用dSpaceCollide函數(shù)獲取可能發(fā)生接觸的物體，再由該函數(shù)指定的碰撞回調(diào)函數(shù)nearCallback將接觸點(diǎn)信息傳給用戶(hù)。由此，用戶(hù)可根據(jù)自身需要建立物體間的碰撞連接點(diǎn)，每個(gè)連接點(diǎn)都有相應(yīng)的dContactGeom 結(jié)構(gòu)，該結(jié)構(gòu)保存著碰撞點(diǎn)的位置和兩個(gè)物體互相進(jìn)入對(duì)方的深度。為了提高仿真速度，軟件使用較少的關(guān)節(jié)和較少的接觸面，如果條件允許，也可以使用無(wú)摩擦或者粘性接觸面。下面給出碰撞回調(diào)函數(shù)中關(guān)于接觸面的參數(shù)設(shè)置：
surface.mu=3.0； //庫(kù)侖摩擦力系數(shù)
surface.mu2=0.0； //庫(kù)侖摩擦力系數(shù)2
surface.slip1=0.05； //摩擦力1方向的滑動(dòng)摩擦力
surface.slip2=0.05； //摩擦力2方向的滑動(dòng)摩擦力
surface.bounce=0.9； //反彈系數(shù)
surface.bounce_vel=1.0； //反彈所需要的最小碰撞速度
surface.soft_erp=0.2； //接觸點(diǎn)法線方向“柔軟”參數(shù)
surface.soft_cfm=1e-4； //接觸點(diǎn)法線方向"柔軟"參數(shù)
       (4)三維圖形繪制
       在仿真軟件中，與繪制圖形有關(guān)的操作函數(shù)都被封裝成動(dòng)態(tài)鏈接庫(kù)，該動(dòng)態(tài)鏈接庫(kù)只向仿真軟件提供若干個(gè)接口，如在繪制機(jī)器人時(shí)，只需根據(jù)機(jī)器人的不同部位，設(shè)定相應(yīng)的繪制參數(shù)，然后輪流調(diào)用dsDrawCylinder、dsDrawBox兩個(gè)函數(shù)即可實(shí)現(xiàn)，dsDrawCylinder函數(shù)的輸入?yún)?shù)分別為物體位置、朝向、長(zhǎng)度和半徑，而dsDrawBox函數(shù)的輸入?yún)?shù)為位置、朝向和尺寸；在繪制從x文件和3ds文件導(dǎo)入的三維模型時(shí)，只需調(diào)用dsDrawTriangleD函數(shù)即可。
4 仿真實(shí)例
       本文在VS2008平臺(tái)上開(kāi)發(fā)仿真軟件，ODE版本為0.9，OpenGL版本為1.0，軟件為控制臺(tái)程序。
       在檢驗(yàn)路徑規(guī)劃算法或避障算法時(shí)，可先創(chuàng)建一個(gè)虛擬足球場(chǎng),再設(shè)定機(jī)器人初始位置的絕對(duì)坐標(biāo)為(-9.0，-5.0)，目的地絕對(duì)坐標(biāo)為(5.5，5.0)，并在路徑的中間布置8個(gè)或更多的障礙物，障礙物直徑為50 cm，其位置參數(shù)可以人為指定，也可以隨機(jī)產(chǎn)生，然后在虛擬場(chǎng)景的二維地圖上，以宏觀鳥(niǎo)瞰的遠(yuǎn)程視野，對(duì)避障算法的仿真結(jié)果進(jìn)行觀察、比較和分析，仿真效果如圖4所示。若需了解算法運(yùn)行的細(xì)節(jié)，可以直接觀察3D環(huán)境的仿真過(guò)程，或通過(guò)分析記錄下的仿真數(shù)據(jù)來(lái)比較算法的優(yōu)劣。而在檢驗(yàn)多機(jī)器人協(xié)作算法時(shí)，可同時(shí)利用三維環(huán)境和二維全局地圖來(lái)對(duì)算法性能進(jìn)行比較，以由多個(gè)移動(dòng)機(jī)器人組成的足球隊(duì)為例，移動(dòng)機(jī)器人需按照協(xié)作算法來(lái)實(shí)現(xiàn)站位[7]。
       此外，在采用PID控制算法對(duì)機(jī)器人位置進(jìn)行控制時(shí)，有比例增益、微分增益和積分增益三個(gè)參數(shù)需要整定測(cè)試[8]，此時(shí),可以利用仿真軟件對(duì)參數(shù)進(jìn)行步估計(jì)。實(shí)驗(yàn)方法是：先設(shè)定初始位置和目的地位置，然后讓機(jī)器人以最快速度向目的地移動(dòng)，當(dāng)?shù)竭_(dá)目的地后，就停止不動(dòng)。在機(jī)器人移動(dòng)過(guò)程中，記錄下機(jī)器人在每一仿真步驟中的位姿(x,y,θ)T，并將位姿數(shù)據(jù)歸一化，以便能夠?qū)⑷S的向量繪制在同一坐標(biāo)系下，同時(shí)記錄機(jī)器人走完設(shè)定路徑所需的時(shí)間。圖5(a)為采用比例控制算法的機(jī)器人位姿變化軌跡，機(jī)器人走完該路徑所需時(shí)間為16.239 s，圖5(b)為采用比例-微分控制算法的變化軌跡，所需時(shí)間為14.026 s。從圖5的結(jié)果可看出，采用比例控制算法的系統(tǒng)輸出無(wú)超調(diào)，而采用比例-微分控制算法的系統(tǒng)，其控制目標(biāo)會(huì)出現(xiàn)微小超調(diào)，但機(jī)器人的響應(yīng)速度明顯提高，尤其是機(jī)器人朝向角的控制，能夠更加快速準(zhǔn)確地跟隨給定值。
移動(dòng)機(jī)器人3D仿真軟件的設(shè)計(jì)與實(shí)現(xiàn)
       在移動(dòng)機(jī)器人控制技術(shù)和多機(jī)器人協(xié)作技術(shù)的研究中，為了能夠?qū)λ惴▍?shù)進(jìn)行有效檢驗(yàn)和測(cè)試，本文利用ODE、OpengGL和VS2008開(kāi)發(fā)出移動(dòng)機(jī)器人仿真軟件。該軟件采用ODE生成動(dòng)力學(xué)世界和模擬物體碰撞，充分利用了ODE的快速性和精確性，仿真軟件還采用高效的圖形接口OpenGL來(lái)繪制圖形，提高軟件的圖形處理能力，改善圖形顯示效果。仿真實(shí)例證明，該軟件具有較好的擴(kuò)展性和實(shí)用性。
參考文獻(xiàn)
[1] 胡宇慧. 3D游戲引擎中若干關(guān)鍵技術(shù)及算法的研究與分析[D]. 上海:上海師范大學(xué), 2006.
[2] 高嵩. 基于OGRE和ODE的駕駛模擬系統(tǒng)的設(shè)計(jì)與實(shí)現(xiàn)[D]. 武漢:武漢理工大學(xué), 2006.
[3] 歐陽(yáng)慧琴, 陳福民. 物理引擎與圖形渲染引擎綁定的研究與實(shí)現(xiàn)[J]. 計(jì)算機(jī)工程與設(shè)計(jì), 2008,29(21):5580-5620.
[4] 蔣東翔. 基于OpenGL的3D網(wǎng)絡(luò)游戲開(kāi)發(fā)中關(guān)鍵技術(shù)的研究[D]. 哈爾濱:哈爾濱工程大學(xué), 2008.
[5] TAKEMURA Y. Development of “Hibikino-Musashi” omnidirectional mobile robot[J/OL]. http://robocup.ksrp.or.jp/hibiki ... 010/05/BrainIT2006_ Ronbun.pdf.
[6] DEMURA K. 3DS Loader for open dynamics engine-0.0[CP/OL]. http://demura.net/9ode/460.html.
[7] 邢關(guān)生, 張建勛. 移動(dòng)機(jī)器人網(wǎng)絡(luò)運(yùn)動(dòng)協(xié)調(diào)的控制與仿真[J]. 系統(tǒng)仿真學(xué)報(bào), 2010,22(03).
[8] 楊智, 朱海鋒, 黃以華. PID控制器設(shè)計(jì)與參數(shù)整定方法綜述[J]. 化工自動(dòng)化及儀表, 2005,32(05):1-7.